#include "LCCGLApi.h"

using namespace LC_CGL_API;
using namespace lc_geometry_mzh;

polygonPoints lc_geometry_mzh::box2Ply(cv::Rect& r)
{
    polygonPoints ply;
    ply.push_back(cv::Point2f(r.x, r.y));
    ply.push_back(cv::Point2f(r.x + r.width, r.y));
    ply.push_back(cv::Point2f(r.x + r.width, r.y + r.height));
    ply.push_back(cv::Point2f(r.x, r.y + r.height));
    return ply;
}

bool lc_geometry_mzh::isPtInPolygon_m(cv::Point2f& pt, polygonPoints& ply)
{
    bool in = false;
    int count = ply.size();
    cv::Point2f p1 = ply[0];
    for (int i = 0; i <= count; i++)
    {
        cv::Point2f p2 = ply[i % count];
        if (pt.y > std::min(p1.y, p2.y))
        {
            if (pt.y < std::max(p1.y, p2.y))
            {
                if (pt.x <= std::max(p1.x, p2.x))
                {
                    if (p1.y != p2.y)
                    {
                        double xinter = (pt.y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y) + p1.x;
                        if (p1.x == p2.x || pt.x <= xinter)
                        {
                            in = !in;
                        }
                    }
                }
            }
        }
        p1.x = p2.x;
        p1.y = p2.y;
    }
    return in;
}


polygonPoints lc_geometry_mzh::getInsidePoints_m(polygonPoints& ply1, polygonPoints& ply2)
{
    polygonPoints insidePoints;
    for (auto& p1 : ply1)
    {
        if (isPtInPolygon_m(p1, ply2))
        {
            insidePoints.push_back(p1);
        }
    }

    for (auto& p2 : ply2)
    {
        if (isPtInPolygon_m(p2, ply1))
        {
            insidePoints.push_back(p2);
        }
    }

    return insidePoints;
}


//denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
bool lc_geometry_mzh::lineIntersection_m(cv::Point2f& pt, polygonPoints& line1, polygonPoints& line2)
{
    bool intersection = false;
    auto p1 = line1[0];
    auto p2 = line1[1];
    auto p3 = line2[0];
    auto p4 = line2[1];
    double d = (p1.x - p2.x) * (p3.y - p4.y) - (p1.y - p2.y) * (p3.x - p4.x);
    if (d == 0)
    {
        return intersection;
    }

    double t = ((p1.x - p3.x) * (p3.y - p4.y) - (p1.y - p3.y) * (p3.x - p4.x)) / d;
    double u = -((p1.x - p2.x) * (p1.y - p3.y) - (p1.y - p2.y) * (p1.x - p3.x)) / d;
    if ((t >= 0 && t <= 1) && (u >= 0 && u <= 1))
    {
        pt.x = p1.x + t * (p2.x - p1.x);
        pt.y = p1.y + t * (p2.y - p1.y);
        intersection = true;
    }

    return intersection;
}

polygonPoints lc_geometry_mzh::getBoxIntersectionPionts_m(polygonPoints& ply1, polygonPoints& ply2)
{
    polygonPoints interPly;
    for (int i = 0; i < 4; i++)
    {
        auto& p1 = ply1[i];
        auto& p2 = ply1[(i + 1) % 4];
        polygonPoints line1{ p1,p2 };
        for (int j = 0; j < 4; j++)
        {
            auto& p3 = ply2[j];
            auto& p4 = ply2[(j + 1) % 4];
            polygonPoints line2{ p3,p4 };
            cv::Point2f p;
            bool inter = lineIntersection_m(p, line1, line2);
            if (inter)
            {
                interPly.push_back(p);
            }
        }
    }

    return interPly;
}


double lc_geometry_mzh::calcPolygonArea_m(polygonPoints& ply)
{
    double area = 0;
    int count = ply.size();
    for (int i = 0; i < count; i++)
    {
        int j = (i + 1) % count;
        auto pi = ply[i];
        auto pj = ply[j];
        area += pi.x * pj.y - pi.y * pj.x;
    }

    return abs(area)/2.0;
}

double lc_geometry_mzh::calcAngle_m(cv::Point2f& ptRef, cv::Point2f& pt)
{
    double a = atan2(pt.y - ptRef.y, pt.x - ptRef.x);
    return a < 0 ? a + 2 * PI : a;
}

polygonPoints lc_geometry_mzh::sortPoinsClockWise(polygonPoints& ply)
{
    // 1.计算外接矩形
    double minX = 100000;
    double maxX = 0;
    double minY = 100000;
    double maxY = 0;
    for (auto& p : ply)
    {
        if (p.x < minX)minX = p.x;
        if (p.x > maxX)maxX = p.x;
        if (p.y < minY)minY = p.y;
        if (p.y > maxY) maxY = p.y;
    }

    // 2.中心点
    cv::Point2f ptCenter((minX + maxX) / 2.0, (minY + maxY) / 2.0);

    // 3.计算所有点到中心点的角度
    //std::vector<double> allAngles;
    struct tempPt
    {
        double ang;
        cv::Point2f pt;
    };
    std::vector<tempPt> allPtWidthAng;
    for (auto& p : ply)
    {
        double a = calcAngle_m(ptCenter, p);
        //allAngles.push_back(a);
        allPtWidthAng.push_back(tempPt{ a,p });
    }

    std::sort(allPtWidthAng.begin(), allPtWidthAng.end(), [&](tempPt& a, tempPt& b) {return a.ang < b.ang; });

    polygonPoints sortPoints;
    for (auto& p : allPtWidthAng)
    {
        sortPoints.push_back(p.pt);
    }

    return sortPoints;
}

// 计算 IOU
double lc_geometry_mzh::calculateIOU(const double box1[4], const double box2[4]) {
    // 为 box1 和 box2 分别定义坐标和尺寸
    double x1 = box1[0], y1 = box1[1], w1 = box1[2], h1 = box1[3];
    double x2 = box2[0], y2 = box2[1], w2 = box2[2], h2 = box2[3];

    // 计算右下角坐标
    double r1x = x1 + w1;
    double r1y = y1 + h1;
    double r2x = x2 + w2;
    double r2y = y2 + h2;

    // 计算交集的左上角和右下角
    double ix1 = std::max(x1, x2);
    double iy1 = std::max(y1, y2);
    double ix2 = std::min(r1x, r2x);
    double iy2 = std::min(r1y, r2y);

    // 计算重叠区域的宽度和高度
    double iw = std::max(0.0, ix2 - ix1);
    double ih = std::max(0.0, iy2 - iy1);

    // 计算重叠面积
    double area_intersection = iw * ih;

    // 计算各个框的面积
    double area1 = w1 * h1;
    double area2 = w2 * h2;

    // 计算并集面积
    double area_union = area1 + area2 - area_intersection;

    // 计算并返回 IOU
    return (area_union > 0) ? (area_intersection / area_union) : 0.0;
}

// 计算目标与区域iou
// obj - 矩形
// region - 任意四边形
double lc_geometry_mzh::calcIOUObjInPolygon(polygonPoints& obj, polygonPoints& region)
{
    auto insidePoints = getInsidePoints_m(obj, region);
    
    auto interPoints = getBoxIntersectionPionts_m(obj, region);
    if (interPoints.size() == 0 && insidePoints.size()==0)
    {
        return 0;
    }
    insidePoints.insert(insidePoints.end(), interPoints.begin(), interPoints.end());
    auto sortPoints = sortPoinsClockWise(insidePoints);

    double area = calcPolygonArea_m(sortPoints);
    double person = calcPolygonArea_m(obj);
    double iou = area / person;
    return iou;
}

double LC_CGL_API::lc_geometry_mzh::calcIOUObjInPolygon(cv::Rect& obj, polygonPoints& region)
{
    auto ply = box2Ply(obj);
    
    return calcIOUObjInPolygon(ply, region);
}
